/*********************************************************************************
 *      Copyright:  (C) 2022 xiezongyi<1764417007@qq.com>
 *                  All rights reserved.
 *
 *       Filename:  tlv_server.c
 *    Description:  This file 
 *                 
 *        Version:  1.0.0(12/08/22)
 *         Author:  xiezongyi <1764417007@qq.com>
 *      ChangeLog:  1, Release initial version on "12/08/22 15:36:21"
 *                 
 ********************************************************************************/
#include <stdio.h>
#include <string.h>

#include "CRC.h"
#include "tlv_server.h"

int parser_tlvpack(char *buf, int bytes, char *tlv_buf, int *tlv_size)
{
    int             tlv_len = 0;
    char            *tlv_ptr = NULL;
    unsigned short  crc16, val;

    if(!buf || !tlv_buf)
    {
        printf("Invalid parameter\n");
        return -1;
    }

praser_again:
    if(bytes < TLV_MIN_SIZE)
    {
        printf("tlv packet too short\n");
        return bytes;
    }

    for(int i=0; i<bytes; i++)
    {
        if((unsigned char)buf[i] == PACK_HEADER)
        {
            if(bytes-i <= 2)
            {
                printf("tlv's value is too short\n");
                memmove(buf, &buf[i], bytes-i);
                return bytes-i;
            }
            tlv_ptr = &buf[i];
            tlv_len = tlv_ptr[2];
            if(tlv_len < TLV_MIN_SIZE || tlv_len > TLV_MAX_SIZE)
            {
                memmove(buf, &tlv_ptr[2], bytes-i-2);
                bytes = bytes-i-2;
                goto praser_again;
            }
            if(tlv_len > bytes-i)
            {
                memmove(buf, tlv_ptr, bytes-i);
                printf("tlv_packet is incomplete\n");
                return bytes-1;
            }

            crc16 = crc16_maxim(tlv_ptr, tlv_len-2);
            val = bytes_to_ushort((unsigned char *)&tlv_ptr[tlv_len-2], 2);

            if(val != crc16)
            {
                printf("CRC check error\n");
                memmove(buf, &tlv_ptr[tlv_len], bytes-i-tlv_len);
                bytes = bytes-i-tlv_len;
                goto praser_again;
            }

            *tlv_size = 0;
            *tlv_size += tlv_len;
            memmove(tlv_buf, tlv_ptr, tlv_len);
            memmove(buf, &tlv_ptr[tlv_len], bytes-i-tlv_len);

            bytes = bytes-i-tlv_len;
            goto praser_again;
        }
    }

    return 0;
}

int bytes_to_ushort(unsigned char *buf, int len)
{
    unsigned short  s;
    unsigned char *ptr = buf;

    for(int i=0; i<(len/2); i++)
    {
         s = (unsigned short)((ptr[i] << 8) + (ptr[i+1] << 0));
    }

    return s;
}

void dump_buf(char *data, int len)
{
    printf("Read %d bytes data from client:\n", len);
    for(int i=0; i<len; i++)
    {
        printf("0x%02X", data[i]);
    }
    printf("\n");
}

